#include "topic_transform_core.h"


TopicTransform::TopicTransform():nh_(""), private_nh_("~"){
    private_nh_.param<std::string>("input_topic", input_topic, "");
    //private_nh_.param<std::string>("input_topic_frame_id", input_topic_frame_id, "");
    private_nh_.param<std::string>("output_topic", output_topic, "");
    private_nh_.param<std::string>("output_topic_frame_id", output_topic_frame_id, "");
    

    
}

TopicTransform::~TopicTransform(){}

void TopicTransform::topic_transform_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
  sensor_msgs::PointCloud2 out = *msg;
  out.header.frame_id = output_topic_frame_id;
  topic_transform_output_pub_.publish(out);
}

void TopicTransform::run(){
    topic_transform_input_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>(input_topic, 1, &TopicTransform::topic_transform_callback, this);
    topic_transform_output_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_topic, 1);
    ros::spin();
}